Dimensionality reduction for point feature SLAM problems with spherical covariance matrices

The main contribution of this paper is the dimensionality reduction for multiple-step 2D point feature based Simultaneous Localization and Mapping (SLAM), which is an extension of our previous work on one-step SLAM (Wang, Huang, Frese & Dissanayake 2013). It has been proved that SLAM with multiple robot poses and a number of point feature positions as variables is equivalent to an optimization problem with only the robot orientations as variables, when the associated uncertainties can be described using spherical covariance matrices. This reduces the dimension of original problem from 3m + 2n to m only (where m is the number of poses and n is the number of features). The optimization problem after dimensionality reduction can be solved numerically using the unconstrained optimization algorithms. While dimensionality reduction may not provide computational saving for all nonlinear optimization problems, for some SLAM problems we can achieve benefits such as improvement on time consumption and convergence. For the special case of two-step SLAM when the orientation information from odometry is not incorporated, an algorithm that can guarantee to obtain the globally optimal solution (in the maximum likelihood sense) is derived. Simulation and experimental datasets are used to verify the equivalence between the reduced nonlinear optimization problem and the original full optimization problem, as well as the proposed new algorithm for obtaining the globally optimal solution for two-step SLAM.